﻿using DroneLib;
using DroneLib.Communication;
using DroneLib.Vehicle;
using System;
using System.Collections.Generic;
using System.Reflection;
using System.Runtime.InteropServices.ComTypes;
using System.Threading;
using System.Threading.Tasks;
using static MAVLink;

namespace DroneLib.Vehicles
{
    public abstract partial class Vehicle:IMessageReciver
    {
        public Guid Guid { get; protected set; } = Guid.NewGuid();
        public string Name { get; set; }
        public bool Active { get; set; } = true;
        public bool Selected { get; set; } = false;
        public byte SysId { get; protected set; }
        public byte GcsId { get; set; } = 255;
        public MAV_TYPE Type { get; protected set; }
        public MAV_AUTOPILOT AutoPilotType { get; protected set; }
        public Firmwares Firmware { get; set; }
        public MAV_STATE Status { get; protected set; }
        protected UInt32 _ModeCode { get; set; }
        public UInt32 ModeCode => _ModeCode;
        public string Mode
        {
            get
            {
                return GetModeName(_ModeCode);
            }
        }
        public abstract bool IsAutoMode { get;}
        public abstract bool IsRtlMode { get; }
        public abstract bool IsAutoTuneMode { get; }
        public abstract bool IsManualMode { get; }
        public int MavlinkVersion { get; protected set; } = 2;
        protected byte _SystemState { get; set; }
        public Version Version { get; protected set; }
        public UInt64 UID { get; protected set; }
        public string UIDStr { get; private set; }
        public MAV_PROTOCOL_CAPABILITY Capabilities { get; private set; }
        //public int TTL { get; protected set; } = 10;
        public TimeSpan TimeSinceLastData => _MavStream is null ? TimeSpan.MaxValue : _MavStream.TimeSinceLastData;
        public double Latitude => _PositionLatLngAlt.Lat;
        public double Longitude => _PositionLatLngAlt.Lng;
        public double Altitude => _PositionLatLngAlt.Alt;
        public double Latitude2 => _PositionLatLngAlt2.Lat;
        public double Longitude2 => _PositionLatLngAlt2.Lng;
        public double Altitude2 => _PositionLatLngAlt2.Alt;

        public bool PositionValid { get; set; } = false;
        public Point3DF PositionNed { get; } = new Point3DF();
        public double Roll => _Attitude.Roll;
        public float Yaw=>_Attitude.Yaw;
        public float Pitch => _Attitude.Pitch;

        public float Heading { get; protected set; }
        public float DistanceToWaypoint => _NavigationInfo.WaypointDistance;
        public int CurrentWaypoint { get; protected set; }
        public int ReachedWaypoint { get; protected set; }
        public bool Armed { get; protected set; }
        public bool SafetySwitch => SystemInfo.SafetyActivated;
        public bool FailSafe =>
            _SystemState == (byte)MAVLink.MAV_STATE.CRITICAL ||
            _SystemState == (byte)MAVLink.MAV_STATE.EMERGENCY ||
            SystemInfo.Sensors.RcReceiverHealth == false || 
            SystemInfo.Sensors.RcReceiverPresent == false;

        public bool RCEnabled => SystemInfo.Sensors.RcReceiverEnabled;
        public bool RCPresent => SystemInfo.Sensors.RcReceiverPresent;
        public MAV_STATE SystemState => (MAV_STATE)_SystemState;
        public int[] RC_Channels { get; } = new int[33];
        public int Throttle { get; protected set; }
        public int RSSI { get; protected set; }
        public PointLatLngAlt HomeLocation { get; } = new PointLatLngAlt();
        public PointLatLngAlt TargetLocation { get; set; }
        public MAVLink.FENCE_BREACH FenceBreachType { get; set; } = FENCE_BREACH.NONE;
        public float OpticalFlow_m_X { get; protected set; }
        public float OpticalFlow_m_Y { get; protected set; }
        public short OpticalFlow_X { get; protected set; }
        public short OpticalFlow_Y { get; protected set; }
        public byte OpticalFlowQuality { get; protected set; }
        public Vibration Vibe { get; } = new Vibration();
        public DateTime GpsTime { get; protected set; } = DateTime.MinValue;
        public EkfInfo EKF { get; } = new EkfInfo();
        public Dictionary<byte, RangeFinder> RangeFinders { get; } = new Dictionary<byte, RangeFinder>()
        {
            {0 , new RangeFinder(0)},
            {1 , new RangeFinder(1)},
            {2 , new RangeFinder(2)}
        };

        public EFI EFI { get; } = new EFI();
        public WindInfo WindInfo { get; } = new WindInfo();
        public SystemInfo SystemInfo { get; } = new SystemInfo();
        public float BoardVoltage => _PowerInfo.BoardVoltage;
        public float Current => _PowerInfo.Current;
        public float Current2 => _PowerInfo.Current2;
        public float BatteryVoltage => _PowerInfo.BatteryInfo.Voltage[0];
        public float BatteryRemaining=>_PowerInfo.BatteryRemaining;

        private List<(DateTime time,MAV_SEVERITY severity, string message)> messages { get; } = new List<(DateTime,MAV_SEVERITY, string)>();
        public PressureInfo PressureInfo { get; } = new PressureInfo();
        public bool UseAirSpeed { get; set; }
        public float AirSpeed => _SpeedInfo.AirSpeed;
        public float GroundSpeed => _SpeedInfo.GroundSpeed[0];
        public float VerticalSpeed => _SpeedInfo.VerticalSpeed;
        public float Course => _SpeedInfo.Course[0];

        public GPS_FIX_TYPE GpsFixType => _GpsInfo.GpsFixType;
        public GPS_FIX_TYPE Gps2FixType => _GpsInfo2.GpsFixType;
        public int SatCount => _GpsInfo.SatCount;
        public int SatCount2 => _GpsInfo2.SatCount;

        private Sensors _Sensors = new Sensors();
        public Sensors Sensors => _Sensors;

        public bool RebootRequired { get; set; } = false;

        //public int HeadingToWp { get; protected set; }
        //public bool MissionValid { get; protected set; }
        //public MISSION_STATE MissionState { get; private set; }
        public RadioInfo RadioInfo { get; } = new RadioInfo();
        public ServoInfo ServoMain { get; } = new ServoInfo();
        public ServoInfo ServoAux { get; } = new ServoInfo();
        public string StatusText { get; protected set; }
        public string LinkName => _MavStream != null ? _MavStream.Name : String.Empty;
        public bool Connected => _MavStream == null ? false : _MavStream.Connected;
        public string TypeName
        {
            get
            {
                switch (Type)
                {

                    case MAVLink.MAV_TYPE.FIXED_WING:
                        {
                            return "固定翼";
                        }
                    case MAVLink.MAV_TYPE.VTOL_FIXEDROTOR:
                    case MAVLink.MAV_TYPE.VTOL_TAILSITTER_DUOROTOR:
                    case MAVLink.MAV_TYPE.VTOL_TAILSITTER_QUADROTOR:
                    case MAVLink.MAV_TYPE.VTOL_TILTROTOR:
                        {
                            return "垂直起降固定翼";
                        }
                    case MAVLink.MAV_TYPE.HELICOPTER:
                        {
                            return "直升机";
                        }
                    case MAVLink.MAV_TYPE.QUADROTOR:
                        {
                            return "四旋翼";
                        }
                    case MAVLink.MAV_TYPE.HEXAROTOR:
                        {
                            return "六旋翼";
                        }
                    case MAVLink.MAV_TYPE.OCTOROTOR:
                        {
                            return "八旋翼";
                        }
                    case MAVLink.MAV_TYPE.DECAROTOR:
                    case MAVLink.MAV_TYPE.DODECAROTOR:
                        {
                            return "多旋翼";
                        }
                    case MAVLink.MAV_TYPE.GROUND_ROVER:
                        {
                            return "车辆";
                        }
                    default:
                        {
                            return "未知";
                        }
                }
            }
        }

        private PointLatLngAlt _movingbase = new PointLatLngAlt();
        public PointLatLngAlt MovingBase
        {
            get => _movingbase;
            set
            {
                if (value == null)
                    _movingbase = new PointLatLngAlt();

                if (_movingbase.Lat != value.Lat || _movingbase.Lng != value.Lng || _movingbase.Alt
                    != value.Alt)
                    _movingbase = value;
            }
        }

        Dictionary<string, mavlink_param_value_t> _Params = new Dictionary<string, mavlink_param_value_t>();
        public Dictionary<string, mavlink_param_value_t> Params => _Params;

        Dictionary<string, mavlink_param_value_t> _ParamBuffer = new Dictionary<string, mavlink_param_value_t>();
        /// <summary>
        /// 参数缓存，当设置参数时，可以先临时保存在此。
        /// </summary>
        public Dictionary<string, mavlink_param_value_t> ParamBuffer => _ParamBuffer;

        public List<mavlink_mission_item_int_t> MissionItems { get; } = new List<mavlink_mission_item_int_t>();

        public object Tag { get; set; }
        public object Tag2 { get; set; }

        bool _VehicleLostTriggered = false;
        protected Vehicle(byte sysId, MavStream link)
        {
            SysId = sysId;
            _MavStream = link;

            if (sysId != 0)
            {
                if (link == null)
                    return;

                LoadMessageProcessMethods();

                link.LinkBroken += Link_LinkBroken;
                new Thread(() =>
                {
                    while (true)
                    {
                        if (TimeSinceLastData.TotalSeconds > 10)
                        {
                            if (_VehicleLostTriggered == false)
                            {
                                _VehicleLostTriggered = true;
                                this.Active = false;
                                this.VehicleLost?.Invoke(this);
                            }
                        }
                        else
                        {
                            if (_VehicleLostTriggered)
                            {
                                _VehicleLostTriggered = false;
                                this.Active = true;
                                VehicleRegained?.Invoke(this);
                            }
                        }
                        
                        Thread.Sleep(1000);
                    }
                })
                { IsBackground = true }
                .Start();

                _Ftp = new MAVFtp(_MavStream, this.SysId, (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1);
            }
        }

        internal static Vehicle Create(byte sysId, MAV_TYPE mavType, MavStream mavStream, UInt32 mode)
        {
            Vehicle v = null;
            switch (mavType)
            {
                case MAV_TYPE.FIXED_WING:
                case MAV_TYPE.VTOL_TILTROTOR:
                case MAV_TYPE.VTOL_FIXEDROTOR:
                case MAV_TYPE.VTOL_TAILSITTER:
                case MAV_TYPE.VTOL_TAILSITTER_DUOROTOR:
                case MAV_TYPE.VTOL_TAILSITTER_QUADROTOR:
                    v = new FixedWing(sysId, mavStream, mavType) { _ModeCode = mode };
                    if (mavStream != null)
                        v.GcsId = mavStream.GcsID;
                    //v.UpdateMode();
                    return v;
                case MAV_TYPE.COAXIAL:
                case MAV_TYPE.DECAROTOR:
                case MAV_TYPE.DODECAROTOR:
                case MAV_TYPE.HEXAROTOR:
                case MAV_TYPE.OCTOROTOR:
                case MAV_TYPE.QUADROTOR:
                case MAV_TYPE.TRICOPTER:
                case MAV_TYPE.GENERIC:
                    v = new MultiRotor(sysId, mavStream, mavType) { _ModeCode = mode };
                    if (mavStream != null)
                        v.GcsId = mavStream.GcsID;
                    //v.UpdateMode();
                    return v;
                case MAV_TYPE.GCS:
                default:
                    return null;
            }
        }

        public Vehicle Clone()
        {
            Vehicle v = Vehicle.Create(this.SysId, this.Type, null, this.ModeCode);
            Type type = this.GetType();
            foreach(PropertyInfo pi in type.GetProperties(BindingFlags.Public | BindingFlags.Instance | BindingFlags.NonPublic))
            {
                if (pi.CanWrite)
                    pi.SetValue(v, pi.GetValue(this));
            }

            foreach (FieldInfo fi in type.GetFields(BindingFlags.Public | BindingFlags.Instance | BindingFlags.NonPublic))
            {
                fi.SetValue(v, fi.GetValue(this));
            }

            return v;
        }

        

        private void Link_LinkBroken(MavStream link)
        {
            this.Active = false;
            if (_VehicleLostTriggered == false)
            {
                _VehicleLostTriggered = true;
                VehicleLost?.Invoke(this);
            }
        }

        public override string ToString()
        {
            return $"{TypeName} - {SysId}";
        }

        protected abstract uint TranslateModeString(string mode);
        public abstract string[] GetModeNames();
        public abstract string GetModeName(UInt32 modeCode);
        public abstract Task<bool> SetMode(string modeName);
        public async Task<bool> SetMode(UInt32 modeCode)
        {
            return await SetModeByModeCode(modeCode);
        }
        //public virtual UInt32 GetModeCode(string modeName)
        //{
        //    return TranslateModeString(modeName);
        //}

        public static UInt32 GetModeCode(Vehicle vehicle, string modeName)
        {
            if (vehicle is FixedWing)
            {
                return ((FixedWing)vehicle).TranslateModeString(modeName);
            }
            else if (vehicle is MultiRotor)
            {
                return ((MultiRotor)vehicle).TranslateModeString(modeName);
            }
            else
                return UInt32.MaxValue;
        }

        public UInt32 GetModeCode(string modeName)
        {
            return TranslateModeString(modeName);
        }
    }
}
